#pragma once
#include <memory>
#include <string>
#include <future>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "fishbot_behavior_tree/async_behavior_base.hpp"
#include "fishbot_behavior_tree/check_error.hpp"


namespace fishbot
{
    namespace behavior_tree
    {
        /**
         * @brief ActionClientBehaviorBase 是一个模板类，继承自 AsyncBehaviorBase，
         *        用于在行为树中实现基于 ROS 2 Action 的异步行为节点。
         *        它封装了 ROS 2 Action 客户端的功能，提供了一个通用的接口，用于发送目标、接收反馈、获取结果以及取消目标
         * 
         * @tparam ActionT 
         */
        template <typename ActionT>
        class ActionClientBehaviorBase : public AsyncBehaviorBase
        {
            using ClientGoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
            /**
             * @brief 定义了一个抽象接口，用于封装 Action 客户端的基本操作
             * 
             */
            class ClientInterfaceBase
            {
            public:
                virtual ~ClientInterfaceBase() = default;

                // Initialize the action client
                virtual void initialize(
                    const std::string &action_name,
                    std::chrono::duration<double> wait_for_server_timeout,
                    std::chrono::duration<double> goal_response_timeout,
                    std::chrono::duration<double> goal_result_timeout,
                    std::chrono::duration<double> cancel_response_timeout) = 0;

                // Wait for the action server to be ready
                virtual bool waitForActionServer() const = 0;

                // Send an action goal and wait for acceptance
                virtual tl::expected<void, std::string> syncSendGoal(
                    const typename ActionT::Goal &goal,
                    typename ClientGoalHandle::FeedbackCallback feedback_fn) = 0;

                // Wait for the result of the action
                virtual tl::expected<typename ClientGoalHandle::WrappedResult, std::string>
                syncGetResult() const = 0;

                // Cancel the goal and wait for confirmation
                virtual tl::expected<std::shared_ptr<typename ActionT::Impl::CancelGoalService::Response>, std::string>
                syncCancelGoal() const = 0;
            };
            /**
             * @brief 实现了 ClientInterfaceBase 接口，基于 ROS 2 的 rclcpp_action 提供具体的功能
             * 
             */
            class RclcppClientInterface : public ClientInterfaceBase
            {
            public:
                explicit RclcppClientInterface(const std::shared_ptr<rclcpp::Node> &node)
                    : node_(node), action_client_(nullptr) {}
                /**
                 * @brief 创建 Action 客户端并设置超时时间
                 * 
                 * @param action_name 
                 * @param wait_for_server_timeout 
                 * @param goal_response_timeout 
                 * @param goal_result_timeout 
                 * @param cancel_response_timeout 
                 */
                void initialize(
                    const std::string &action_name,
                    std::chrono::duration<double> wait_for_server_timeout,
                    std::chrono::duration<double> goal_response_timeout,
                    std::chrono::duration<double> goal_result_timeout,
                    std::chrono::duration<double> cancel_response_timeout) override
                {
                    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
                    wait_for_server_timeout_ = wait_for_server_timeout;
                    goal_response_timeout_ = goal_response_timeout;
                    goal_result_timeout_ = goal_result_timeout;
                    cancel_response_timeout_ = cancel_response_timeout;
                }
                /**
                 * @brief 等待 Action 服务端就绪
                 * 
                 * @return true 
                 * @return false 
                 */
                bool waitForActionServer() const override
                {
                    return action_client_->wait_for_action_server(wait_for_server_timeout_);
                }
                /**
                 * @brief 发送目标并等待服务端接受
                 * 
                 * @param goal 
                 * @param feedback_fn 
                 * @return tl::expected<void, std::string> 
                 */
                tl::expected<void, std::string> syncSendGoal(
                    const typename ActionT::Goal &goal,
                    typename ClientGoalHandle::FeedbackCallback feedback_fn) override
                {
                    auto goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
                    goal_options.feedback_callback = feedback_fn;

                    auto future_goal_handle = action_client_->async_send_goal(goal, goal_options);
                    if (rclcpp::spin_until_future_complete(node_, future_goal_handle, goal_response_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
                    {
                        return tl::unexpected<std::string>("Failed to send goal");
                    }
                    else
                    {
                        RCLCPP_INFO(rclcpp::get_logger("ActionClientBehaviorBase"), "Send goal request accepted");
                    }

                    auto goal_handle = future_goal_handle.get();
                    if (!goal_handle)
                    {
                        return tl::unexpected<std::string>("Goal was rejected by the server");
                    }

                    goal_handle_ = goal_handle;
                    return {};
                }
                /**
                 * @brief 获取目标的执行结果
                 * 
                 * @return tl::expected<typename ClientGoalHandle::WrappedResult, std::string> 
                 */
                tl::expected<typename ClientGoalHandle::WrappedResult, std::string> syncGetResult() const override
                {
                    if (!goal_handle_)
                    {
                        return tl::unexpected<std::string>("No goal handle available");
                    }

                    auto future_result = action_client_->async_get_result(goal_handle_);
                    if (rclcpp::spin_until_future_complete(node_, future_result, goal_result_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
                    {
                        return tl::unexpected<std::string>("Failed to get result");
                    }

                    return future_result.get();
                }

                tl::expected<std::shared_ptr<typename ActionT::Impl::CancelGoalService::Response>, std::string> syncCancelGoal() const override
                {
                    if (!goal_handle_)
                    {
                        return tl::unexpected<std::string>("No goal handle available to cancel");
                    }

                    auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
                    if (rclcpp::spin_until_future_complete(node_, future_cancel, cancel_response_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
                    {
                        return tl::unexpected<std::string>("Failed to cancel goal");
                    }

                    return future_cancel.get();
                }

            private:
                std::shared_ptr<rclcpp::Node> node_;
                typename rclcpp_action::Client<ActionT>::SharedPtr action_client_;
                typename ClientGoalHandle::SharedPtr goal_handle_;

                std::chrono::duration<double> wait_for_server_timeout_;
                std::chrono::duration<double> goal_response_timeout_;
                std::chrono::duration<double> goal_result_timeout_;
                std::chrono::duration<double> cancel_response_timeout_;
            };

        public:
            // Constructor using RclcppClientInterface
            ActionClientBehaviorBase(const std::string &name, const BT::NodeConfiguration &config,
                                     const std::shared_ptr<BehaviorContext> &shared_resources)
                : AsyncBehaviorBase(name, config, shared_resources),
                  client_interface_(std::make_unique<RclcppClientInterface>(shared_resources->node_))
            {
            }

            virtual ~ActionClientBehaviorBase() = default;
            /**
             * @brief 实现异步任务的核心逻辑
             *        获取 Action 名称和超时时间。
             *         初始化 Action 客户端。
             *         等待服务端就绪。
             *         创建并发送目标。
             *         等待目标执行完成并获取结果。
             *         根据结果返回成功或失败。
             * 
             * @return tl::expected<bool, std::string> 
             */
            // Required implementation of BT::StatefulActionNode::onStart()
            tl::expected<bool, std::string> doWork() override
            {
                // Initialize action client
                auto action_name = getActionName();
                if (!action_name)
                {
                    return tl::make_unexpected(action_name.error());
                }

                auto wait_for_server_timeout = getServerTimeout();
                auto goal_response_timeout = getResponseTimeout();
                auto goal_result_timeout = getResultTimeout();
                auto cancel_response_timeout = getCancelTimeout();

                client_interface_->initialize(action_name.value(), wait_for_server_timeout.value(), goal_response_timeout.value(), goal_result_timeout.value(), cancel_response_timeout.value());

                if (!client_interface_->waitForActionServer())
                {
                    RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Action server not available");
                    return tl::make_unexpected("Action server is not ready: " + action_name.value());
                }

                auto goal = createGoal();
                if (!goal)
                {
                    RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Failed to create goal");
                    return tl::make_unexpected("Failed to create action goal request");
                }
                else
                {
                    RCLCPP_INFO(rclcpp::get_logger("ActionClientBehaviorBase"), "Created goal");
                }

                try
                {
                    // Send the goal to the action server
                    auto response = client_interface_->syncSendGoal(goal.value(), std::bind(&ActionClientBehaviorBase::processFeedback, this, std::placeholders::_1, std::placeholders::_2));
                    if (!response)
                    {
                        RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Failed to send action goal request");
                        return tl::make_unexpected("Failed to send action goal request");
                    }

                    // 等待动作完成
                    auto result = client_interface_->syncGetResult();
                    if (!result)
                    {
                        RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Failed to get action result");
                        return tl::make_unexpected("Failed to get action result");
                    }
                    auto wrapped_result = result.value();
                    if (wrapped_result.code != rclcpp_action::ResultCode::SUCCEEDED)
                    {
                        std::ostringstream oss;
                        oss << "Failed to finish result, error with code: " << int(wrapped_result.code);
                        RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), oss.str().c_str());
                        return tl::make_unexpected(oss.str());
                    }
                    else
                    {
                        processResult(wrapped_result.result);
                    }
                }
                catch (const std::exception &e)
                {
                    RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Exception: %s", e.what());
                    return tl::make_unexpected("Action failed with exception: " + std::string(e.what()));
                }

                return true;
            }

            // Required implementation of BT::StatefulActionNode::onHalted()
            void onHalted() override
            {
                // Handle halt behavior, e.g., cancel goal or cleanup
                client_interface_->syncCancelGoal();
            }

            // // Resets the internal status of this node
            // void resetStatus() override
            // {
            //     result_future_ = std::future<std::shared_ptr<typename ActionT::Result>>();
            // }

        protected:
            // User-provided function to get the name of the action when initializing the action client
            virtual tl::expected<std::string, std::string> getActionName()
            {
                const auto action_name = getInput<std::string>("action_name");
                if (const auto error = fishbot::behavior_tree::maybe_error(action_name))
                {
                    RCLCPP_ERROR(rclcpp::get_logger("ActionClientBehaviorBase"), "Input action_name is missing");
                    return tl::make_unexpected("Input action_name is missing");
                }
                return action_name.value();
            };

            // Optional user-provided function to set the timeout used when waiting for the action server
            virtual tl::expected<std::chrono::duration<double>, std::string> getServerTimeout()
            {
                auto timeout = getInput<double>("wait_for_server_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ActionClientBehaviorBase"), "Input wait_for_server_timeout is missing, set to default 5s");
                    return std::chrono::duration<double>(5.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            // Optional user-provided function to set the timeout used when waiting for the goal send response
            virtual tl::expected<std::chrono::duration<double>, std::string> getResponseTimeout()
            {
                auto timeout = getInput<double>("goal_response_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ActionClientBehaviorBase"), "Input goal_response_timeout is missing, set to default 5s");
                    return std::chrono::duration<double>(5.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            // Optional user-provided function to set the timeout used when waiting for the action result
            virtual tl::expected<std::chrono::duration<double>, std::string> getResultTimeout()
            {
                auto timeout = getInput<double>("goal_result_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ActionClientBehaviorBase"), "Input goal_result_timeout is missing, set to default 600s");
                    return std::chrono::duration<double>(600.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            // Optional user-provided function to set the timeout used when waiting for the action goal cancel response
            virtual tl::expected<std::chrono::duration<double>, std::string> getCancelTimeout()
            {
                auto timeout = getInput<double>("cancel_response_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ActionClientBehaviorBase"), "Input cancel_response_timeout is missing, set to default 60s");
                    return std::chrono::duration<double>(60.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            // User-provided function to create the action goal before sending the action goal request
            virtual tl::expected<typename ActionT::Goal, std::string> createGoal() = 0;

            // Optional user-provided function to process the action result after the action has finished
            virtual tl::expected<bool, std::string> processResult(const std::shared_ptr<typename ActionT::Result> result)
            {
                return true; // Default behavior: always return true
            }

            // Optional user-provided function to process feedback sent by the action server
            virtual void processFeedback(const typename ClientGoalHandle::SharedPtr result, const std::shared_ptr<const typename ActionT::Feedback> feedback)
            {
                // Default behavior: do nothing
            }

            // Optional user-provided function to retrieve and surface an error message from an aborted action server result
            virtual std::string getAbortedMessage(const std::shared_ptr<const typename ActionT::Result> result) const
            {
                return "Action aborted"; // Default message
            }

            std::shared_future<tl::expected<bool, std::string>> &getFuture() override
            {
                return future_;
            }
            std::shared_future<tl::expected<bool, std::string>> future_;

        private:
            std::unique_ptr<ClientInterfaceBase> client_interface_;
            std::future<std::shared_ptr<typename ActionT::Result>> result_future_;
        };
    } // namespace behavior_tree
} // namespace fishbot

